// ==============================================================================================
// BSD 3-Clause Clear License
// Copyright © 2025 ZAMA. All rights reserved.
// ----------------------------------------------------------------------------------------------
// ucore firmware
//
// Continuously:
// *  Read IOPs from work queue
// *  Parse them to extract OpCode and src/dest ops
// *  Read corresponding DOps stream from translation table
// *  If DOp src/dst match template pattern, patch Opcode with real IOps args
// *  Send patched DOps to HW
//
// Behind the scene, an interrupt handler wait on Hw DOps ack and forward them to host through
// the ack queue.
// ==============================================================================================

#include "ucore.h"

// Global variables
// ============================================================================================= //
AckIopLut_t ack_iop_lut;

// Op parsing and patching functions prototypes
// ============================================================================================= //
void parse_iop(
     // Static allocated buffer used during IOp parsing
     IOpHeader_t *header,
     IOpOperand_t *operand,
     IOpImmHeader_t *imm_header,
     // Operand/Immediat bundle generated by the parser
     OperandBundle_t* dst,
     OperandBundle_t* src,
     ImmediatBundle_t* imm);
void get_lookup(IOpHeader_t header, Lookup_t* lookup);

// IRQ constant, global variables and function prototypes
// ============================================================================================= //
#define INTC_HANDLER    XIntc_InterruptHandler
#define INTC_DEVICE_ID  XPAR_INTC_0_DEVICE_ID
XIntc InterruptController;

// Ack_forward
// Read DOp ack, match with associated pending IOp and forward the ack to host
void irqx_dop_ack_fwd(void);
int irqx_init(void);
void exception_init(void);

#define DOP_BUFFER_LOG2_SIZE 8
#define DOP_BUFFER_SIZE (1 << DOP_BUFFER_LOG2_SIZE)

// Firmware main function
// ============================================================================================= //
int main () {
  uint32_t dop_buffer[DOP_BUFFER_SIZE];
  // Initialization of the ublaze
  // ------------------------------------------------------------------------------------------- //
  Xil_ICacheEnable();
  Xil_DCacheEnable();
  irqx_init();
  exception_init();

  // Init global variables
  init(&ack_iop_lut);

  // Loop body
  // ------------------------------------------------------------------------------------------- //
  while(1) {
    // Various structure used by iop parser
    IOpHeader_t header;
    IOpOperand_t operand;
    IOpImmHeader_t imm_header;
    OperandBundle_t dst_bundle;
    OperandBundle_t src_bundle;
    ImmediatBundle_t imm_bundle;

    Lookup_t dop_entry;
    DOpu_t   dop;
    size_t   memcpy_size;
    size_t   chunk_index=0;
    uint32_t iop_ack;

    // Read iop and parse it and store in lookup for ack
    parse_iop(&header, &operand, &imm_header, &dst_bundle, &src_bundle, &imm_bundle);
    push(&ack_iop_lut, header.raw);

    // processing of read & write IOp does not use DOp tables
    if (header.header.opcode != UCORE_VERSION_IOPCODE) {
        // Get associated translation unit
        get_lookup(header, &dop_entry);
        // Copy a chunk of DOP (not more than dop_buffer size)
        memcpy_size = (dop_entry.len > DOP_BUFFER_SIZE) ? DOP_BUFFER_SIZE : dop_entry.len;
        Xil_MemCpy(dop_buffer, (uint32_t*)(DOP_LUT_ADDR + dop_entry.offset), memcpy_size * sizeof(uint32_t));

        // Patch and stream DOps to HW
        for (int i=0; i< dop_entry.len; i++) {
          // if the dop_buffer is done we need to trigger another memcpy
          if ((i>>DOP_BUFFER_LOG2_SIZE) > chunk_index) {
              memcpy_size = ((dop_entry.len-i) > DOP_BUFFER_SIZE) ?
                      DOP_BUFFER_SIZE : (dop_entry.len-i);
              Xil_MemCpy(dop_buffer, (uint32_t*)(DOP_LUT_ADDR + dop_entry.offset + (i*sizeof(uint32_t))), memcpy_size * sizeof(uint32_t));
              chunk_index+=1;
          }

          dop.raw = dop_buffer[i%DOP_BUFFER_SIZE];
          patch_dop(&dop, &dst_bundle, &src_bundle, &imm_bundle);
          putfsl_interruptible(dop.raw, DOP_MAXIS_ID);
        }
        // Add DOp sync
        putfsl_interruptible(SYNC_DOP_WORD, DOP_MAXIS_ID);
        // It is not clear why this is needed
        Xil_DCacheFlush();
    } else {
      // like any other IOp we ack with 1st work of input IOp
      iop_ack = pop(&ack_iop_lut);
      iop_ack += ((UCORE_VERSION_MAJOR << 8) + UCORE_VERSION_MINOR) & 0xFFFF;
      putfsl(iop_ack, ACKQ_MAXIS_ID);
    }
  }

  // Code teardown
  // NB: In practice, this is deadcode since we used an infinite body loop
  // ------------------------------------------------------------------------------------------- //
  Xil_DCacheDisable();
  Xil_ICacheDisable();

  return XST_SUCCESS;
}

// Ops functions body
// ============================================================================================= //
// Parse an IOp from workq stream
// Currently there is no way to report error back to host -> No check are implemented during parsing
// TODO implement error return through ACKQ and implement check during parsing
void parse_iop(
     // Static allocated buffer used during IOp parsing
     IOpHeader_t *header,
     IOpOperand_t *operand,
     IOpImmHeader_t *imm_header,
     // Operand/Immediat bundle generated by the parser
     OperandBundle_t* dst,
     OperandBundle_t* src,
     ImmediatBundle_t* imm) {

  //1. Get header
  getfsl_interruptible(header->raw, WORKQ_SAXIS_ID);

  //2. Get list of destination operands
  uint32_t dst_pos = 0;
  do {
    getfsl_interruptible(operand->raw, WORKQ_SAXIS_ID);

    // Fill current slot
    dst->operand[dst_pos].cid_ofst = operand->operand.base_cid;
    dst->operand[dst_pos].len = operand->operand.vec_size +1;
    dst_pos +=1;
  } while (!operand->operand.is_last);

  // Fill bundle length
  dst->len = dst_pos;


  //3. Get list of source operands
  uint32_t src_pos = 0;
  do {
    getfsl_interruptible(operand->raw, WORKQ_SAXIS_ID);

    // Fill current slot
    src->operand[src_pos].cid_ofst = operand->operand.base_cid;
    src->operand[src_pos].len = operand->operand.vec_size +1;
    src_pos +=1;
  } while (!operand->operand.is_last);

  // Fill bundle length
  src->len = src_pos;

  //4. Get list of immediat operands (if needed)
  if (header->header.has_imm) {
    uint32_t imm_pos = 0;
    do {
      // Read Imm header
      getfsl_interruptible(imm_header->raw, WORKQ_SAXIS_ID);

      // Extract lsb from header
      imm->cst[imm_pos].msg[0] = imm_header->header.lsb_msg;

      uint32_t msg_pos = 1;
      while ( imm_header->header.block > ((8*sizeof(uint16_t))/MSG_WIDTH)*msg_pos) {
        uint32_t bfr;
        getfsl_interruptible(bfr, WORKQ_SAXIS_ID);

        // Fill current slot
        imm->cst[imm_pos].msg[msg_pos] = bfr & 0xffff;
        imm->cst[imm_pos].msg[msg_pos+1] = (bfr>> 16) & 0xffff;
        msg_pos +=2;
      };
      // Fill immediat length
      imm->cst[imm_pos].len = msg_pos;

      imm_pos += 1;
    } while (!imm_header->header.is_last);
    // Fill immediat bundle length
    imm->len = imm_pos;
  }
}

void get_lookup(IOpHeader_t header, Lookup_t* lookup){

  // Read translation offset for the given entry
  // Offset is computed based on max blk_width to correctly handle asym IOp such as Cmp
  uint8_t max_align = (header.header.dst_align > header.header.src_align)? header.header.dst_align: header.header.src_align;
  uintptr_t entry_addr = (uintptr_t) (DOP_LUT_ADDR + ((DOP_LUT_RANGE *max_align) + (header.header.opcode)) * sizeof(uint32_t));
  size_t entry = Xil_In32(entry_addr);

  // NB: each translation slot start with translation unit length
  lookup->len = Xil_In32(DOP_LUT_ADDR + entry);
  lookup->offset = entry + sizeof(uint32_t);
}

// Irq functions body
// ============================================================================================= //
// Ack_forward
// Read DOp ack, match with associated pending IOp and forward the ack to host
void irqx_dop_ack_fwd(void) {
  uint32_t dop_ack, iop_ack;

  getfsl(dop_ack, ACK_SAXIS_ID);
  for (int ack = 0; ack < dop_ack; ack++) {
    iop_ack = pop(&ack_iop_lut);
    putfsl(iop_ack, ACKQ_MAXIS_ID);
  }
}

int irqx_init(void){
  int status;

  // Enable irq controller and check status
  microblaze_enable_interrupts();
  status = XIntc_Initialize(&InterruptController, INTC_DEVICE_ID);

  // TODO add check for status and handle it

  // Register handler and enable associated line
  status = XIntc_Connect(
    &InterruptController,
    XPAR_UBLAZE_0_AXI_INTC_SYSTEM_IRQ_0_INTR,
    (XInterruptHandler) irqx_dop_ack_fwd,
    (void*) 0
  );
  XIntc_Enable(&InterruptController, XPAR_UBLAZE_0_AXI_INTC_SYSTEM_IRQ_0_INTR);

  // Start Irq handling
  XIntc_Start(&InterruptController, XIN_REAL_MODE);
  XIntc_MasterEnable((uint32_t)&InterruptController);

  return status;
}

void exception_init(void) {
  Xil_ExceptionInit();
  Xil_ExceptionRegisterHandler(XIL_EXCEPTION_ID_M_AXI_I_EXCEPTION, (XExceptionHandler)INTC_HANDLER, &InterruptController);

  Xil_ExceptionEnable();
}
